#include "control_base/WMJRobotControl.h"

namespace wmj
{
    void WMJRobotControl::SendVisionCurrentStatue(ROBO_STATE Mode, CAMERA_EXPOSURE exposure, bool auto_shoot_ok, int id,
                                                  bool ifrecognize, bool light_ok, bool light_compare_ok, bool number_ok,
                                                  int cameranum, int erroritem, bool armor_compare_ok, bool select_ok)
    {
        VSMPack control_data{};
        Buffer data_to_send{};

        // std::cout << "auto_shoot:" << auto_shoot_ok << std::endl
        //           << "recognize:" << ifrecognize << std::endl
        //           << "light:" << light_ok << std::endl
        //           << "light_compare:" << light_compare_ok << std::endl
        //           << "numdetect:" << number_ok << std::endl
        //           << "doubleye:" << cameranum << std::endl
        //           << "errorcamera:" << erroritem << std::endl
        //           << "armor_compare:" << armor_compare_ok << std::endl
        //           << "select:" << select_ok << std::endl;

        uint8_t auto_shoot = 0x01 & auto_shoot_ok;

        uint8_t recognize = 0x01 & ifrecognize;
        uint8_t light = 0x01 & light_ok;
        uint8_t light_compare = 0x01 & light_compare_ok;
        uint8_t numdetect = 0x01 & number_ok;
        uint8_t doubleye = 0x01 & (cameranum >> 1);
        uint8_t errorcamera = 0x01 & erroritem;
        uint8_t armor_compare = 0x01 & armor_compare_ok;
        uint8_t select = 0x01 & select_ok;

        control_data.msg.vision_Mode = (uint8_t)Mode | exposure << 3 | auto_shoot << 5;
        control_data.msg.vision_Armorid = (uint8_t)id;
        control_data.msg.vision_Recognize = recognize | light << 1 | light_compare << 2 | numdetect << 3 |
                                            doubleye << 4 | errorcamera << 5 | armor_compare << 6 | select << 7;
        data_to_send.push_back((uint8_t)Vision);
        for (auto c : control_data.data)
            data_to_send.push_back(c);

        sendFrame(data_to_send);
        // canBus->sendFrame(data_to_send);
    }
    void WMJRobotControl::CameraTrigger(bool v_trigger_enable, int v_delay_time, int v_rate)
    {
        std::cout << "start trigger camera" << std::endl;
        VSMPack control_data{};
        Buffer data_to_send{};
        control_data.msg.vision_CameraTriggerEnable = v_trigger_enable;
        control_data.msg.vision_CameraRate = v_rate;
        control_data.msg.vision_CameraDlay = v_delay_time;
        data_to_send.push_back((uint8_t)Vision);
        for (auto c : control_data.data)
            data_to_send.push_back(c);
        sendFrame(data_to_send);
    }

}